#include "Defines.inc"

Global Integer Status
Global Integer Status_temp
Function Main
 	Call RobotInit
 	Call SelectProgram
Fend



Function SelectProgram
	Do
	Status = In(STATUS_IN)
	If Sw(POWER_MODE) Then
		Power High
	Else
		Power Low
	EndIf
	If Status_temp = Status Then
		GoTo Ext99
	EndIf
	Status_temp = Status
	Speed In(SPEED_GLOBAL)
	Select Status
		Print "#Status_IN:", Status
		Case 99
			Call PrePick
		Case 1
			Call PICK_UP
		Case 2
			Call Pick
		Case 3
			Call PreFlux
		Case 4
			Call Flux_UP
		Case 5
			Call Flux
		Case 6
			Call PreSolder
		Case 7
			Call Solder_UP
		Case 9
            Call Solder
		Case 12
			Call PreInspection
		Case 13
			Call Inspection(1)
		Case 14
			Call Inspection(2)
		Case 15
			Call Inspection(3)
		Case 16
			Call Inspection(4)
		Case 17
			Call Inspection(5)
		Case 18
			Call Inspection(6)
		Case 19
			Call Inspection(7)
		Case 20
			Call Inspection(8)
		Case 21
			Call Inspection(9)
		Case 22
			Call Inspection(10)
		Case 23
			Call Inspection(11)
		Case 24
			Call Inspection(12)
		Case 26
			Call PreDorpOK
		Case 27
			Call DorpOKLeft
		Case 28
			Call DorpOKRight
		Case 29
			Call DropNGLeft
		Case 30
			Call DropNGRight
		Case 31
			Call preDorpNg
					
	Send
	Print "#STATUS_OUT", Status
	Out STATUS_OUT, Status
Ext99:
	Loop
Fend

Function PrePick
	Routine(GoSafePoint, 0)
	Go PTPREPICK
Fend

Function Pick
	If IsPickID Then
		Go PTPICK +Z(D_UP)
		SetSpeed(IDX_SPEED_PICK)
	    Go PTPICK
	    SetSpeed(IDX_SPEED_GLOBAL)
	Else
		Print "position error"
	EndIf
Fend

Function PICK_UP
	If IsPickID Then
		SetSpeed(IDX_SPEED_PICK)
		Go PTPICK +Z(D_UP)
	    SetSpeed(IDX_SPEED_GLOBAL)
	Else
		Print "position error"
    EndIf
Fend

Function PreFlux
	Routine(GoSafePoint, 1)
	Jump PTPREFLUX
Fend

Function Flux
	If IsFluxID Then
		Go PTflux +Z(D_UP)
		SetSpeed(IDX_SPEED_FLUX)
		Go PTFLUX
	    SetSpeed(IDX_SPEED_GLOBAL)
	Else
		Print "position error"
	EndIf
Fend


Function Flux_UP
	If IsFluxID Then
		SetSpeed(IDX_SPEED_FLUX)
		Go PTflux +Z(D_UP)
	    SetSpeed(IDX_SPEED_GLOBAL)
	Else
		Print "position error"
	EndIf
Fend

Function PreSolder
	Routine(GoSafePoint, 2)
	Go PTPRESOLDER
Fend

Function Solder_UP
	If isSolderID Then
		SetSpeed(IDX_SPEED_SOLDER)
		Go PTSOLDER +Z(D_UP)
	    SetSpeed(IDX_SPEED_GLOBAL)
	 Else
	 	Print "position error"
	EndIf
Fend
Function Solder
	If isSolderID Then
		Go PTSOLDER +Z(D_UP)
		SetSpeed(IDX_SPEED_SOLDER)
		Go PTSOLDER
	    SetSpeed(IDX_SPEED_GLOBAL)
	 Else
	 	Print "position error"
	EndIf
Fend
Function PreInspection
	Routine(GoSafePoint, 3)
	Go PTPREINSPECTION
Fend
Function Inspection(index As Integer)
	If isInspectionID = False Then
		Call PreInspection
	EndIf
	Go P(INSPECTION1_ID + index - 1)
Fend

Function PreDorpOK
	Routine(GoSafePoint, -1)
    Go PTPREOK
Fend

Function preDorpNg
	If isDropNGID = False Then
   		Routine(GoSafePoint, -2)
   		Pass PTPRENG
	EndIf
	Jump PTPRENG
Fend

Function DorpOKLeft
	If isDropOKID = False Then
		Routine(GoSafePoint, -1)
		Pass PTPREOK
	EndIf
   	Jump Pallet(1, GetPalletNum)
Fend
Function DorpOKRight
	If isDropOKID = False Then
		Routine(GoSafePoint, -1)
		Pass PTPREOK
	EndIf
    Jump Pallet(2, GetPalletNum)
Fend

Function DropNGLeft
	If isDropNGID = False Then
   		Routine(GoSafePoint, -2)
   		Pass PTPRENG
	EndIf
   Jump PTDROPNGleft
Fend
Function DropNGRight
	If isDropNGID = False Then
   		Routine(GoSafePoint, -2)
   		Pass PTPRENG
	EndIf
   Jump PTDROPNGRight
Fend
Function Routine(source As Integer, dest As Integer)
	P(198) = PTPRENG
	P(199) = PTPREOK
	P(200) = PTPrePick
	P(201) = ptpreflux
	P(202) = ptpresolder
	P(203) = ptpreinspection
	Integer i
	If source < dest Then
	For i = source To dest
			Pass P(200 + i)
	Next
	Else
	For i = source To dest Step -1
			Pass P(200 + i)
	Next
	EndIf
Fend

Function GoSafePoint() As Integer
	Integer j, k
	j = GetCurrentPointID
	If j = PREPICK_ID Or j = PICK_ID Then
		SetSpeed(IDX_SPEED_PICK)
		Pass PTPREPICK
		k = 0
	ElseIf j = PREFLUX_ID Or j = FLUX_ID Then
		SetSpeed(IDX_SPEED_FLUX)
		Pass PTPREFLUX
		k = 1
 	ElseIf j = PRESOLDER_ID Or j = SOLDER_ID Then
 		SetSpeed(IDX_SPEED_SOLDER)
 		Pass PTPRESOLDER
 		k = 2
 	ElseIf isInspectionID Then
 		Pass PTPREINSPECTION
		k = 3
	ElseIf isDropOKID Then
		Jump PTPREOK
		k = -1
	ElseIf isDropNGID Then
		Jump PTPRENG
		k = -2
	EndIf
GoSafePoint = k
Fend
